package de.hhn.mim.psb;

import static org.junit.Assert.*;

import org.junit.After;
import org.junit.AfterClass;
import org.junit.Before;
import org.junit.BeforeClass;
import org.junit.Test;


/**
 * Modul LMRegistTests
 * implementiert eine Testfunktion zur Kontrolle der
 * Berechnung von Transformationen
 * Sie ist so konzipiert, um Kombinationen aller moeglicher
 * Rotationswinkel zu testen. Wenn der Test fehlschlaegt, 
 * sollten sinnvollerweise eigene Testfunktionen implementiert
 * werden, die einfache Transformationen verwenden, deren
 * Auswirkung leicht vorhergesagt werden kann.
 *
 * Die implementierten Methoden habe print flags, die 
 * abhaengig von ihrem Wert unterschiedliche Ausgaben veranlassen
 * Bitte passen Sie diese Flags Ihren Beduerfnissen an
 *
 * @author Rolf Bendl
 *
 */
public class LMRegistTests
 {
  @BeforeClass
  public static void setUpBeforeClass() throws Exception
   {
   }

  @AfterClass
  public static void tearDownAfterClass() throws Exception
   {
   }

  @Before
  public void setUp() throws Exception
   {
   }

  @After
  public void tearDown() throws Exception
   {
   }

  @Test
  
  public void testCalcTrafo() throws Exception
  {
   Vect3D p1 = new Vect3D(100, 0, 0);
   Vect3D p2 = new Vect3D(0, 100, 0);
   Vect3D p3 = new Vect3D(0, 0, 0);
   testCalcTrafo1(p1, p2, p3);
  }
 
 /**
  * ruft fuer alle Winkel 0 <= X <= 181, 0 <= Y <= 181, 0 <= Z <= 181
  * die Testfunktion test1 auf, diese transformiert die Punkte um die Winkel,
  * berechnet eine R�cktransformation und vergleicht die zurueck 
  * transformierten mit den Originalkoordinaten
  *
  * Achtung: 
  * Die Methode ist so konzipiert, dass sie die Rotationsparameter
  * ueber den gesamten Winkelbereich variiert, damit sicher gestellt ist,
  * dass alle Winkelkombinationen getestet werden.
  * Zur Fehlersuche ist es sinnvoll, die Bereiche zu beschneiden
  * z.B. 
  *   x = 1; x <= 1 und y = 0; y < = 0; z = 0; z <= 0
  *   oder Winkel, bei denen das Ergebnis leicht vorhersagbar ist
  *   (45�, 90�, ..)
  * 
  * @throws Exception 
  */
 public void testCalcTrafo1(Vect3D v1, Vect3D v2, Vect3D v3) throws Exception
  {
   for(int x = 0; x <= 181; x++)
    {
     double rx = (double) x;
     for(int y = 0; y <= 181; y++)
      {
       double ry = (double) y;
       for(int z = 0; z <= 181; z++)
        {
         double  rz    = (double) z;
         int     print = 0;
         testCalcTrafo2(v1, v2, v3, rx, ry, rz, print);
        }         
      }
    }
  }
  
 /**
  * erhaelt drei Punkte und drei Drehwinkel. Die Punkte werden um x-, y- und 
  * z-Achse rotiert. Anschliessend werden die Original- und die transformierten 
  * Punkte der Methode calcTrafo uebergeben und die (inverse) Transformation 
  * berechnet, mit der die transformierten auf die Originalkoordinaten
  * abgebildet werden. Wenn der Parameter print = true ist, werden die
  * Transformationsmatrizen und das Ergebnis der Hin-Transformation ausgegeben.
  * @throws Exception 
  */
 public void testCalcTrafo2(Vect3D v1, Vect3D v2, Vect3D v3,
                            double rx, double ry, double rz,
                            int print) throws Exception
  {
   Vect3D [] floating = new Vect3D [3];
   Vect3D [] fixed    = new Vect3D [3];

   fixed[0]    = v1;
   fixed[1]    = v2;
   fixed[2]    = v3;
   floating[0] = v1;
   floating[1] = v2;
   floating[2] = v3;
   
   Vect3D trans    = new Vect3D(0, 0, 0);
   Vect3D trans2   = new Vect3D(0, 0, 0);
   Matrix3D t1Mat  = Matrix3D.translation(trans);
   Matrix3D trafo1 = new Matrix3D(1);
   Matrix3D rx1Mat = Matrix3D.rotationX(Math.toRadians(rx));
   Matrix3D ry1Mat = Matrix3D.rotationY(Math.toRadians(ry));
   Matrix3D rz1Mat = Matrix3D.rotationZ(Math.toRadians(rz));
   
   trafo1 = t1Mat;
   Vect3D [] rTrans = new Vect3D[floating.length];
   if(print > 1) System.out.println("Translation");
   for(int i = 0; i < floating.length; i++)
    {
     rTrans[i] = trafo1.mult(floating[i]);
     if(print > 1) rTrans[i].print();
    }
   if(print > 1) System.out.println();
   trafo1 = rx1Mat;
   Vect3D [] rRx = new Vect3D[floating.length];
   if(print > 1) System.out.println("RotationX");
   for(int i = 0; i < floating.length; i++)
    {
     rRx[i] = trafo1.mult(floating[i]);
     if(print > 1) rRx[i].print();
    }
   if(print > 1) System.out.println();
   trafo1 = ry1Mat;
   Vect3D [] rRy = new Vect3D[floating.length];
   if(print > 1) System.out.println("RotationY");
   for(int i = 0; i < floating.length; i++)
    {
     rRy[i] = trafo1.mult(floating[i]);
     if(print > 1) rRy[i].print();
    }
   if(print > 1) System.out.println();
   trafo1 = rz1Mat;
   Vect3D [] rRz = new Vect3D[floating.length];
   if(print > 1) System.out.println("RotationZ");
   for(int i = 0; i < floating.length; i++)
    {
     rRz[i] = trafo1.mult(floating[i]);
     if(print > 1) rRz[i].print();
    }
   if(print > 1) System.out.println();
   
   trafo1 = rz1Mat.mult(t1Mat); 
   Vect3D [] rRzT = new Vect3D[floating.length];
   if(print > 1) System.out.println("RotationZ - Translation");
   for(int i = 0; i < floating.length; i++)
    {
     rRzT[i] = trafo1.mult(floating[i]);
     if(print > 1) rRzT[i].print();
    }
   
   trafo1.identity();
   trafo1 = trafo1.mult(t1Mat);
   if(print > 1) t1Mat.print("tMat");
   if(print > 1) trafo1.print("tMat * trafo");
   trafo1 = trafo1.mult(rx1Mat);
   if(print > 1) trafo1.print("rxMat * tMat * trafo");
   trafo1 = trafo1.mult(ry1Mat);
   if(print > 1) trafo1.print("ryMat * rxMat * tMat * trafo");    
   trafo1 = trafo1.mult(rz1Mat);
   if(print > 1) trafo1.print("rzMat * ryMat * rxMat * tMat * trafo");

   Vect3D [] result = new Vect3D[floating.length];
   if(print > 1) System.out.println("RotZ - RotY - RotX - Translation");

   Matrix3D trafo2 = new Matrix3D(1);
   Matrix3D t2Mat  = Matrix3D.translation(trans2);
   Matrix3D rx2Mat = Matrix3D.rotationX(Math.toRadians(-rx));
   Matrix3D ry2Mat = Matrix3D.rotationY(Math.toRadians(-ry));
   Matrix3D rz2Mat = Matrix3D.rotationZ(Math.toRadians(-rz));
   
   trafo2.identity();
   trafo2 = trafo2.mult(t2Mat);
   if(print > 1) t2Mat.print("t2Mat");
   if(print > 1) trafo2.print("t2Mat * trafo");
   trafo2 = trafo2.mult(rx2Mat);
   if(print > 1) trafo2.print("rxMat * t2Mat * trafo");
   trafo2 = trafo2.mult(ry2Mat);
   if(print > 1) trafo2.print("ryMat * rxMat * t2Mat * trafo");    
   trafo2 = trafo2.mult(rz2Mat);
   if(print > 1) trafo2.print("rzMat * ryMat * rxMat * t2Mat * trafo");
   for(int i = 0; i < fixed.length; i++)
    {
     result[i] = trafo2.mult(fixed[i]);
     if(print > 0) result[i].print("fixed[" + i + "]   ");
     fixed[i] = result[i];      
    }
   for(int i = 0; i < floating.length; i++)
    {
     result[i] = trafo1.mult(floating[i]);
     if(print > 0) result[i].print("floating[" + i + "]");
     floating[i] = result[i];      
    }
   if(print > 0) System.out.println();
   
   LMRegist.LMPair [] lms = new LMRegist.LMPair[3];
   for(int i = 0; i < 3; i++)
   lms[i] = new LMRegist.LMPair(fixed[i], floating[i]);

   int myPrint = 0;
   Matrix3D resMat = LMRegist.calcTrafo(lms[0].first, lms[1].first, lms[2].first,
                               lms[0].second, lms[1].second, lms[2].second,
                               myPrint);
   if(resMat == null)
    {
    }
   Vect3D r0 = new Vect3D();
   Vect3D r1 = new Vect3D();
   Vect3D r2 = new Vect3D();
   
   r0 = resMat.mult(lms[0].second);
   r1 = resMat.mult(lms[1].second);
   r2 = resMat.mult(lms[2].second);
   
   boolean equal0 = r0.equals(lms[0].first);
   boolean equal1 = r1.equals(lms[1].first);
   boolean equal2 = r2.equals(lms[2].first);
   
   if(!equal0 | !equal1 | !equal2)
    {
     Exception e = new Exception("Test1 fehlgeschlagen bei rX " + rx + 
                                 " rY " + ry + " rZ " + rz);
     throw(e);
    }
  }

 }
